The provided dataset represents the motion from the two wheels of a thymio, as measured through a pretty inaccurate method based on the observed voltages of the two motors (i.e. not a wheel encoder).
Each 0.1s, you get a reading that represents the total amount of rotation of the left and right wheels, since an arbitrary starting point in the past; the value corresponds to the total displacement, in meters, at the edge of each wheel (this already takes into account the wheel radius). The distance between the wheels is $2l = 9.35$ cm.
# Preparation
import pandas as pd
import numpy as np
np.set_printoptions(precision=4, suppress=True)
import matplotlib.pyplot as plt
import ipywidgets
def mktr(x, y):
return np.array([[1, 0, x],
[0, 1, y],
[0, 0, 1]])
def mkrot(theta):
return np.array([[np.cos(theta), -np.sin(theta), 0],
[np.sin(theta), np.cos(theta), 0],
[0, 0, 1]])
def drawf(f, ax=None, name=None):
""" Draw frame defined by f on axis ax (if provided) or on plt.gca() otherwise """
xhat = f @ np.array([[0, 0, 1], [1, 0, 1]]).T
yhat = f @ np.array([[0, 0, 1], [0, 1, 1]]).T
if(not ax):
ax = plt.gca()
ax.plot(xhat[0, :], xhat[1, :], 'r-') # transformed x unit vector
ax.plot(yhat[0, :], yhat[1, :], 'g-') # transformed y unit vector
if(name):
ax.text(xhat[0, 0], xhat[1, 0], name, va="top", ha="center")
def drawrobot(f, l, ax=None, alpha=0.5):
""" Draw robot at f, with wheel distance from center l,
on axis ax (if provided) or on plt.gca() otherwise.
if l is None, no wheels are drawn"""
if(not ax):
ax = plt.gca()
robot = ([[-1, 2, -1, -1], # x
[-1, 0, 1, -1]]) # y
robot = np.array(robot)
robot = np.vstack((
robot * 0.1, # scale by 0.1 units
np.ones((1, robot.shape[1]))))
robott = f @ robot
wheell = np.array([
[-0.05, 0.05],
[l, l],
[1, 1]
])
wheelr = wheell * np.array([[1, -1, 1]]).T
wheellt = f @ wheell
wheelrt = f @ wheelr
ax.plot(robott[0, :], robott[1, :], 'k-', alpha=alpha)
ax.plot(wheellt[0, :], wheellt[1, :], 'k-', alpha=alpha)
ax.plot(wheelrt[0, :], wheelrt[1, :], 'k-', alpha=alpha)
def ddtr(vl, vr, l, dt):
""" returns the pose transform for a motion with duration dt of a differential
drive robot with wheel speeds vl and vr and wheelbase l """
if(np.isclose(vl, vr)): # we are moving straight, R is at the infinity and we handle this case separately
return mktr((vr + vl)/2*dt, 0) # note we translate along x ()
omega = (vr - vl) / (2 * l) # angular speed of the robot frame
R = l * (vr + vl) / (vr - vl)
# Make sure you understand this!
return mktr(0, R) @ mkrot(omega * dt) @ mktr(0, -R)
df = pd.read_csv("data.csv")
print(df.tail())
fig,ax = plt.subplots()
ax.plot(df["t"], df["l_disp"], label="l_disp")
ax.plot(df["t"], df["r_disp"], label="r_disp")
ax.set(xlabel = "time [s]")
ax.legend();
t l_disp r_disp 299 29.9 -9.058860 -8.372935 300 30.0 -9.058860 -8.372935 301 30.1 -9.059159 -8.372935 302 30.2 -9.059159 -8.372935 303 30.3 -9.059159 -8.372935
Observe that:
Let's now compute the tangential velocities of the two wheels, in m/s, and move to numpy arrays, that you might be more familiar with.
df[["l_vel","r_vel"]] = df[["l_disp","r_disp"]].diff(axis=0).fillna(0) / 0.1
l_vel, r_vel = df[["l_vel","r_vel"]].values.T
t = df["t"].values
You can now work with 1d numpy arrays l_vel (m/s), r_vel (m/s), t (s)
# Visualize to make sure the data is sane
fig,ax = plt.subplots()
ax.plot(t, l_vel, label="left")
ax.plot(t, r_vel, label="right")
ax.axhline(y=0, color="k")
ax.legend()
ax.set(xlabel="time [s]", ylabel="wheel tangential velocity [m/s]");
This will require to implement the robot kinematics. You can visualize the trajectory of the robot reference point as a line (note: you won't see the orientation of the robot!).
As a sanity check: the reference point of the robot moves within a small area (approximately $12 \times 10$ centimeters); the robot often turns in place, don't expect a long smooth trajectory.
Ts=[np.eye(3)]
l=4.675/100.0
dt=0.1
x_array=[]
y_array=[]
for v_l,v_r in zip(df["l_vel"],df["r_vel"]):
#print(v_r, v_l)
Ts.append(Ts[-1] @ ddtr(v_l, v_r, l, dt))
xy=Ts[-1][:2, 2]
x=xy[0]
y=xy[1]
x_array.append(x)
y_array.append(y)
#print(Ts[-1][:2, 2])
plt.plot(x_array, y_array)
x_array_no_noise=np.array(x_array)
y_array_no_noise=np.array(y_array)
print(abs(min(x_array)-max(x_array)))
print(abs(min(y_array)-max(y_array)))
0.12465572688629434 0.11044769401634917
%matplotlib notebook
import matplotlib
Ts=[np.eye(3)]
l=4.675/100.0
dt=0.1
x_array=[]
y_array=[]
for v_l,v_r in zip(df["l_vel"],df["r_vel"]):
#print(v_r, v_l)
Ts.append(Ts[-1] @ ddtr(v_l, v_r, l, dt))
xy=Ts[-1][:2, 2]
x=xy[0]
y=xy[1]
x_array.append(x)
y_array.append(y)
#lines=(np.interp(np.linspace(0,30),x_array[-2:], y_array[-2:] ))
#print(Ts[-1][:2, 2])
#lines=(np.interp(np.linspace(0,30),x_array[-2:], y_array[-2:] ))
x_array=np.array(x_array)
y_array=np.array(y_array)
plt.rcParams["animation.html"] = "jshtml"
plt.rcParams['figure.dpi'] = 150
plt.ioff()
meters=0.0
def animate(t):
plt.cla()
global meters
plt.plot(x_array[t],x_array[t], marker='*')
plt.xlim(min(x_array)-0.05,max(x_array)+0.06)
plt.ylim(min(y_array)-0.01,max(y_array)+0.01)
if(t!=0):
a = np.array(( x_array[t-1],y_array[t-1]))
b = np.array(( x_array[t],y_array[t]))
meters+=np.linalg.norm(a-b)
meters_s=str(np.round(meters, decimals=2))
plt.plot(x_array[:t+1], y_array[:t+1], color='red')
plt.grid()
plt.xlabel('$x$')
plt.ylabel('$y$')
plt.gca().set_aspect('equal', adjustable='box')
#plt.legend(['time step: '+str(t)])
plt.annotate('time step: '+str(t)+'\npath: '+meters_s+' m', xy=(max(x_array)-0.02, max(y_array)-0.02))
if(t!=len(x_array)-1 or t!=0):
#length_arrow=0.06
plt.arrow(x_array[t], y_array[t], x_array[t]-x_array[t-1], y_array[t]-y_array[t-1], width=0.003, head_width=0.006, head_length=0.005, head_starts_at_zero=True)
#plt.autoscale()
%matplotlib notebook
fig, ax = plt.subplots()
from matplotlib import animation
import time
start=time.time()
anim=animation.FuncAnimation(fig, animate, frames=len(x_array),interval=100 )
end=time.time()
print(f"time for creating animation: {end-start} secs")
start=time.time()
anim.save('anim1_no_noise.mp4')
end=time.time()
print(f"time for saving animation: {end-start} secs")
anim
time for creating animation: 0.0002498626708984375 secs time for saving animation: 6.378216981887817 secs